#include "hal_device.h"
#include "hal_sensor.h"
#include "stm32f4xx.h"
#include "stm32f4xx_ll_tim.h"
#include "stm32f4xx_ll_bus.h"
#include "stm32f4xx_ll_gpio.h"


static int stm32f4xx_pwm_gpio_init(struct hal_dev_s *dev)
{
	LL_GPIO_InitTypeDef GPIO_InitStruct = {0};
	
	LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB);
	LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOC);
	LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOD);
	LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOE);
	
	/** GPIO Configuration  
	PE11   ------> TIM1_CH2     // servo 1
	PE9    ------> TIM1_CH1     // servo 2
	
	PE13   ------> PHA          // servo 1
	PE14   ------> PHB          // servo 2
	
	PB0    ------> TIM3_CH3     // main motor
	PC8    ------> TIM8_CH3     // tail motor pwm
	
	PA4    ------> EN           // servo
	
	PD14   ------> PHC          // tail motor direction
	*/
	GPIO_InitStruct.Pin        = LL_GPIO_PIN_11|LL_GPIO_PIN_9;
	GPIO_InitStruct.Mode       = LL_GPIO_MODE_ALTERNATE;
	GPIO_InitStruct.Speed      = LL_GPIO_SPEED_FREQ_HIGH;
	GPIO_InitStruct.Pull       = LL_GPIO_PULL_UP;
	GPIO_InitStruct.Alternate  = LL_GPIO_AF_1;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	LL_GPIO_Init(GPIOE, &GPIO_InitStruct);
	
	GPIO_InitStruct.Pin        = LL_GPIO_PIN_13|LL_GPIO_PIN_14;
	GPIO_InitStruct.Mode       = LL_GPIO_MODE_OUTPUT;
	GPIO_InitStruct.Speed      = LL_GPIO_SPEED_FREQ_HIGH;
	GPIO_InitStruct.Pull       = LL_GPIO_PULL_NO;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	LL_GPIO_Init(GPIOE, &GPIO_InitStruct);
	
	
	GPIO_InitStruct.Pin        = LL_GPIO_PIN_14;
	GPIO_InitStruct.Mode       = LL_GPIO_MODE_OUTPUT;
	GPIO_InitStruct.Speed      = LL_GPIO_SPEED_FREQ_HIGH;
	GPIO_InitStruct.Pull       = LL_GPIO_PULL_NO;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	LL_GPIO_Init(GPIOD, &GPIO_InitStruct);
	
	GPIO_InitStruct.Pin        = LL_GPIO_PIN_4;
	GPIO_InitStruct.Mode       = LL_GPIO_MODE_OUTPUT;
	GPIO_InitStruct.Speed      = LL_GPIO_SPEED_FREQ_HIGH;
	GPIO_InitStruct.Pull       = LL_GPIO_PULL_NO;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	LL_GPIO_Init(GPIOA, &GPIO_InitStruct);
	
	GPIO_InitStruct.Pin        = LL_GPIO_PIN_0;
	GPIO_InitStruct.Mode       = LL_GPIO_MODE_ALTERNATE;
	GPIO_InitStruct.Speed      = LL_GPIO_SPEED_FREQ_HIGH;
	GPIO_InitStruct.Pull       = LL_GPIO_PULL_UP;
	GPIO_InitStruct.Alternate  = LL_GPIO_AF_2;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	LL_GPIO_Init(GPIOB, &GPIO_InitStruct);

	GPIO_InitStruct.Pin        = LL_GPIO_PIN_8;
	GPIO_InitStruct.Mode       = LL_GPIO_MODE_ALTERNATE;
	GPIO_InitStruct.Speed      = LL_GPIO_SPEED_FREQ_LOW;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	GPIO_InitStruct.Pull       = LL_GPIO_PULL_NO;
	GPIO_InitStruct.Alternate  = LL_GPIO_AF_3;

	LL_GPIO_Init(GPIOC, &GPIO_InitStruct);
	
	return 0;
}
	
static int stm32f4xx_pwm_tim1_init(struct hal_dev_s *dev)
{
	LL_TIM_InitTypeDef TIM_InitStruct = {0};
	LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};
	LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = {0};
	
	/* Peripheral clock enable */
	LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM1);

	TIM_InitStruct.Prescaler = 7;
	TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
	TIM_InitStruct.Autoreload = 1000;
	TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
	TIM_InitStruct.RepetitionCounter = 0;
	LL_TIM_Init(TIM1, &TIM_InitStruct);
	LL_TIM_DisableARRPreload(TIM1);
	
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1);
	
	TIM_OC_InitStruct.OCMode       = LL_TIM_OCMODE_PWM1;
	TIM_OC_InitStruct.OCState      = LL_TIM_OCSTATE_DISABLE;
	TIM_OC_InitStruct.OCNState     = LL_TIM_OCSTATE_DISABLE;
	TIM_OC_InitStruct.CompareValue = 0;
	TIM_OC_InitStruct.OCPolarity   = LL_TIM_OCPOLARITY_HIGH;
	TIM_OC_InitStruct.OCNPolarity  = LL_TIM_OCPOLARITY_LOW;
	TIM_OC_InitStruct.OCIdleState  = LL_TIM_OCIDLESTATE_LOW;
	TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_HIGH;
	LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct);
	LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1);
	
	LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2);
	LL_TIM_OC_Init(TIM1, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct);
	LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2);
	
	LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET);
	LL_TIM_DisableMasterSlaveMode(TIM1);
	TIM_BDTRInitStruct.OSSRState = LL_TIM_OSSR_DISABLE;
	TIM_BDTRInitStruct.OSSIState = LL_TIM_OSSI_DISABLE;
	TIM_BDTRInitStruct.LockLevel = LL_TIM_LOCKLEVEL_OFF;
	TIM_BDTRInitStruct.DeadTime = 0;
	TIM_BDTRInitStruct.BreakState = LL_TIM_BREAK_DISABLE;
	TIM_BDTRInitStruct.BreakPolarity = LL_TIM_BREAK_POLARITY_HIGH;
	TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE;
	LL_TIM_BDTR_Init(TIM1, &TIM_BDTRInitStruct);
	return 0;
}

static int stm32f4xx_pwm_tim3_init(struct hal_dev_s *dev)
{
	LL_TIM_InitTypeDef TIM_InitStruct = {0};
	LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};
	LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = {0};
	
	/* Peripheral clock enable */
	LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);

	TIM_InitStruct.Prescaler         = 83;
	TIM_InitStruct.CounterMode       = LL_TIM_COUNTERMODE_UP;
	TIM_InitStruct.Autoreload        = 2500;
	TIM_InitStruct.ClockDivision     = LL_TIM_CLOCKDIVISION_DIV1;
	TIM_InitStruct.RepetitionCounter = 0;
	LL_TIM_Init(TIM3, &TIM_InitStruct);
	LL_TIM_DisableARRPreload(TIM3);
	
	LL_TIM_OC_EnablePreload(TIM3, LL_TIM_CHANNEL_CH3);
	
	TIM_OC_InitStruct.OCMode       = LL_TIM_OCMODE_PWM1;
	TIM_OC_InitStruct.OCState      = LL_TIM_OCSTATE_DISABLE;
	TIM_OC_InitStruct.OCNState     = LL_TIM_OCSTATE_DISABLE;
	TIM_OC_InitStruct.CompareValue = 1000;
	TIM_OC_InitStruct.OCPolarity   = LL_TIM_OCPOLARITY_HIGH;
	TIM_OC_InitStruct.OCNPolarity  = LL_TIM_OCPOLARITY_LOW;
	TIM_OC_InitStruct.OCIdleState  = LL_TIM_OCIDLESTATE_LOW;
	TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_HIGH;
	LL_TIM_OC_Init(TIM3, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct);
	LL_TIM_OC_DisableFast(TIM3, LL_TIM_CHANNEL_CH3);
	
	LL_TIM_SetTriggerOutput(TIM3, LL_TIM_TRGO_RESET);
	LL_TIM_DisableMasterSlaveMode(TIM3);
	
	TIM_BDTRInitStruct.OSSRState   = LL_TIM_OSSR_DISABLE;
	TIM_BDTRInitStruct.OSSIState   = LL_TIM_OSSI_DISABLE;
	TIM_BDTRInitStruct.LockLevel   = LL_TIM_LOCKLEVEL_OFF;
	TIM_BDTRInitStruct.DeadTime    = 0;
	TIM_BDTRInitStruct.BreakState  = LL_TIM_BREAK_DISABLE;
	TIM_BDTRInitStruct.BreakPolarity   = LL_TIM_BREAK_POLARITY_HIGH;
	TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE;
	LL_TIM_BDTR_Init(TIM3, &TIM_BDTRInitStruct);
	
	return 0;
}

static int stm32f4xx_pwm_tim8_init(struct hal_dev_s *dev)
{
	LL_TIM_InitTypeDef TIM_InitStruct = {0};
	LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};
	LL_TIM_BDTR_InitTypeDef TIM_BDTRInitStruct = {0};
	
	/* Peripheral clock enable */
	LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM8);

	TIM_InitStruct.Prescaler         = 7;
	TIM_InitStruct.CounterMode       = LL_TIM_COUNTERMODE_UP;
	TIM_InitStruct.Autoreload        = 1000;
	TIM_InitStruct.ClockDivision     = LL_TIM_CLOCKDIVISION_DIV1;
	TIM_InitStruct.RepetitionCounter = 0;
	LL_TIM_Init(TIM8, &TIM_InitStruct);
	LL_TIM_DisableARRPreload(TIM8);
	
	LL_TIM_OC_EnablePreload(TIM8, LL_TIM_CHANNEL_CH3);
	
	TIM_OC_InitStruct.OCMode       = LL_TIM_OCMODE_PWM1;
	TIM_OC_InitStruct.OCState      = LL_TIM_OCSTATE_DISABLE;
	TIM_OC_InitStruct.OCNState     = LL_TIM_OCSTATE_DISABLE;
	TIM_OC_InitStruct.CompareValue = 1000;
	TIM_OC_InitStruct.OCPolarity   = LL_TIM_OCPOLARITY_HIGH;
	TIM_OC_InitStruct.OCNPolarity  = LL_TIM_OCPOLARITY_HIGH;
	TIM_OC_InitStruct.OCIdleState  = LL_TIM_OCIDLESTATE_LOW;
	TIM_OC_InitStruct.OCNIdleState = LL_TIM_OCIDLESTATE_LOW;
	LL_TIM_OC_Init(TIM8, LL_TIM_CHANNEL_CH3, &TIM_OC_InitStruct);
	LL_TIM_OC_DisableFast(TIM8, LL_TIM_CHANNEL_CH3);
	
	LL_TIM_SetTriggerOutput(TIM8, LL_TIM_TRGO_RESET);
	LL_TIM_DisableMasterSlaveMode(TIM8);
	TIM_BDTRInitStruct.OSSRState       = LL_TIM_OSSR_DISABLE;
	TIM_BDTRInitStruct.OSSIState       = LL_TIM_OSSI_DISABLE;
	TIM_BDTRInitStruct.LockLevel       = LL_TIM_LOCKLEVEL_OFF;
	TIM_BDTRInitStruct.DeadTime        = 0;
	TIM_BDTRInitStruct.BreakState      = LL_TIM_BREAK_DISABLE;
	TIM_BDTRInitStruct.BreakPolarity   = LL_TIM_BREAK_POLARITY_HIGH;
	TIM_BDTRInitStruct.AutomaticOutput = LL_TIM_AUTOMATICOUTPUT_DISABLE;
	LL_TIM_BDTR_Init(TIM8, &TIM_BDTRInitStruct);
	
	return 0;
}


static int stm32f4xx_pwm_init(struct hal_dev_s *dev)
{
	stm32f4xx_pwm_tim1_init(dev);
	stm32f4xx_pwm_tim3_init(dev);
	stm32f4xx_pwm_tim8_init(dev);
	stm32f4xx_pwm_gpio_init(dev);
	return 0;
}

static int stm32f4xx_pwm_open(struct hal_dev_s *dev,uint16_t flag)
{
	LL_TIM_EnableCounter(TIM1);
	LL_TIM_CC_EnableChannel(TIM1,LL_TIM_CHANNEL_CH1);
	LL_TIM_CC_EnableChannel(TIM1,LL_TIM_CHANNEL_CH2);
	
	LL_TIM_EnableCounter(TIM3);
	LL_TIM_CC_EnableChannel(TIM3,LL_TIM_CHANNEL_CH3);
	
	LL_TIM_EnableCounter(TIM8);
	LL_TIM_CC_EnableChannel(TIM8,LL_TIM_CHANNEL_CH3);
	
	LL_TIM_EnableAllOutputs(TIM1);
	LL_TIM_EnableAllOutputs(TIM3);
	LL_TIM_EnableAllOutputs(TIM8);
	
	LL_GPIO_SetOutputPin(GPIOA,LL_GPIO_PIN_4);
	LL_GPIO_ResetOutputPin(GPIOD,LL_GPIO_PIN_14);
	
	return 0;
}

static int stm32f4xx_pwm_write(struct hal_dev_s *dev, const void *buffer, int buflen,int pos)
{
	uint32_t dutycycle[4];
	struct motor_data_s *motor = (struct motor_data_s *)buffer;
	
	if(buflen != sizeof(struct motor_data_s)){
		return -1;
	}
	
	if(motor->channel[2] < 0.f){
		if(motor->channel[2] < -1.0f){
			motor->channel[2] = -1.0f;
		}
		LL_GPIO_SetOutputPin(GPIOE,LL_GPIO_PIN_13);
		dutycycle[2] = (uint32_t)(motor->channel[2] * -1000.f);
		LL_TIM_OC_SetCompareCH1(TIM1,dutycycle[2]);
	}else {
		if(motor->channel[2] > 1.0f){
			motor->channel[2] = 1.0f;
		}
		LL_GPIO_ResetOutputPin(GPIOE,LL_GPIO_PIN_13);
		dutycycle[2] = (uint32_t)(motor->channel[2] * 1000.f);
		LL_TIM_OC_SetCompareCH1(TIM1,dutycycle[2]);
	}
	
	if(motor->channel[3] < 0.f){
		if(motor->channel[3] < -1.0f){
			motor->channel[3] = -1.0f;
		}
		dutycycle[3] = (uint32_t)(motor->channel[3] * -1000.f);
		LL_TIM_OC_SetCompareCH2(TIM1,dutycycle[3]);
		LL_GPIO_SetOutputPin(GPIOE,LL_GPIO_PIN_14);
	}else {
		if(motor->channel[3] > 1.0f){
			motor->channel[3] = 1.0f;
		}
		dutycycle[3] = (uint32_t)(motor->channel[3] * 1000.f);
		LL_TIM_OC_SetCompareCH2(TIM1,dutycycle[3]);
		LL_GPIO_ResetOutputPin(GPIOE,LL_GPIO_PIN_14);
	}
	
	if(motor->channel[0] < 0.01f){
		dutycycle[0] = 1050;
	}else if(motor->channel[0] > 1.f ){
		dutycycle[0] = 2000;
	}else{
		dutycycle[0] = (uint32_t)(motor->channel[0] * 950.f) + 1050;
	}
	
	if(motor->channel[1] < 0.01f){
		dutycycle[1] = 0;
	}else if(motor->channel[1] > 1.f ){
		dutycycle[1] = 1000;
	}else{
		dutycycle[1] = (uint32_t)(motor->channel[1] * 1000.f);
	}
	
	LL_TIM_OC_SetCompareCH3(TIM3,dutycycle[0]);
	LL_TIM_OC_SetCompareCH3(TIM8,dutycycle[1]);
	
	return buflen;
}


struct hal_dev_s stm32f4xx_pwm_helicopter_dev;

int stm32f4xx_helicopter_pwm_register(void)
{
	stm32f4xx_pwm_helicopter_dev.init  = stm32f4xx_pwm_init;
	stm32f4xx_pwm_helicopter_dev.open  = stm32f4xx_pwm_open;
	stm32f4xx_pwm_helicopter_dev.write = stm32f4xx_pwm_write;
	
	hal_dev_register(&stm32f4xx_pwm_helicopter_dev,"pwm",HAL_O_WRONLY);
	return 0;
}
